Aruco Tag Guided Drawing Robot

$ sudo apt-get install python-opencv

$ sudo apt-get install python3-opencv

Import cv2
import numpy as np
import cv2.aruco as aruco


# Load the predefined dictionary
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) #size is 4 by 4 and number is 50 - faster


def aruco_create():
   for i in range(50):
       # Generate the markers
       outputMarker = np.zeros((200, 200), dtype=np.uint8)
       markerImage = cv2.aruco.drawMarker(dictionary, i, 500, outputMarker, 1) #500 is the number of pixels
       image_path = r'/home/pi/project/aruco/4X4Marker%i.jpg' %i
       cv2.imwrite(image_path , markerImage)


aruco_create()

After printing the markers, we used the following code to detect the markers. It is important to note that there should be a white offsetted background around the markers for the true detection. Corners of the markers are stored in a list based on the order shown in the photo:

0: top left corner

1: top right corner

2: bottom right corner

3: bottom left corner

This is important because the perspective correction calculations and robot pose estimation, specifically the direction of the robot are based on the corners and the orientation of the marker. Many errors can occur simply because of lack of attention to the correct positioning of the marker.

Import cv2
import numpy as np
import cv2.aruco as aruco

#Aruco detection
cap = cv2.VideoCapture(0)

while True:
   ret, frame = cap.read()
   gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

   aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
   arucoParameters = aruco.DetectorParameters_create()
   corners, ids, rejectedImgPoints = aruco.detectMarkers(
       gray, aruco_dict, parameters=arucoParameters)
   print(ids)
   if np.all(ids):
     image = aruco.drawDetectedMarkers(frame,corners,ids)         
     cv2.imshow('Display', image)
   else:
       display = frame
       cv2.imshow('Display', display)
      
   if cv2.waitKey(1) & 0xFF == ord('q'):
       break

cap.release()
cv2.destroyAllWindows()

import cv2
import numpy as np
import cv2.aruco as aruco
import time
cap = cv2.VideoCapture(0)
last_recorded_time = time.time()
frame_count = 0

while True: 
 _, frame = cap.read()
 curr_time = time.time()
 cv2.imshow('Display', frame)
 if (curr_time - last_recorded_time >= 2.0):
   cv2.imwrite('frame_{}.png'.format(frame_count), frame)
   last_recorded_time = curr_time
   frame_count+=1
 if cv2.waitKey(1) & 0xFF == ord('q'):
   break

cap.release()
cv2.destroyAllWindows()

import numpy as np
import cv2
import glob

# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
# size of each square is 35 mm
square_size = 0.0355 #in meters
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = (square_size * np.mgrid[0:9,0:6]).T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

images = glob.glob(r'/home/pi/project/calib_photos/*.png',)

for fname in images:
   img = cv2.imread(fname)
   gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
   sample_im = gray
   #cv2.imshow(gray)
   # Find the chess board corners
   chessboard_flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
   flags = 0
   flags |= cv2.CALIB_CB_ADAPTIVE_THRESH
   flags |= cv2.CALIB_CB_NORMALIZE_IMAGE
   [ret, corners] = cv2.findChessboardCorners(gray, (9, 6), flags)

   # If found, add object points, image points (after refining them)
   if (ret == True):
       #print ('True')
       objpoints.append(objp)
       corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
       imgpoints.append(corners2) #these are the found corners

       # Draw and display the corners
       img = cv2.drawChessboardCorners(img, (9,6), corners2,ret)
       cv2.imshow('img',img)
       cv2.waitKey(100)

cv2.destroyAllWindows()
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)


# Undistortion
# refine the camera matrix
ii = -1
for fname in images:
   #if (fname==images[0]):
   ii +=1
   img = cv2.imread(fname)
   h,  w = img.shape[:2]
   newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))
   dst1 = cv2.undistort(img, newcameramtx, dist, None, None)
  
   # testing if the matrix is right
   image_path1 = r'/home/pi/project/calib_photos/final/%i.png' % ii
   cv2.imwrite(image_path1, dst1)

path = r'/home/pi/project/picamera.yml'
cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE)
cv_file.write("new_matrix", newcameramtx)
cv_file.write("distortion_coef", dist)

cv_file.release()

# Re-projection Error
tot_error = 0
for i in range(len(objpoints)):
   imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
   error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
   tot_error += error

print ("total error: ", tot_error/len(objpoints))

%YAML:1.0
---
new_matrix: !!opencv-matrix
  rows: 3
  cols: 3
  dt: d
  data: [ 5.0729638671875000e+02, 0., 3.1174656415419304e+02, 0.,
      5.0761346435546875e+02, 2.3997776397076632e+02, 0., 0., 1. ]
distortion_coef: !!opencv-matrix
  rows: 1
  cols: 5
  dt: d
  data: [ 1.9076841407258338e-01, -4.5190192083740538e-01,
      2.7079765950399783e-04, 1.3991690924355539e-04,
      2.7966157704609523e-01 ]

[0.0, 0.0], [worldx, 0.0], [0.0, worldy], [worldx, worldy]

import socket
import cv2
import numpy as np
import datetime
import cv2.aruco as aruco
import glob
import math
import faulthandler; faulthandler.enable()
import multiprocessing as mp
import time

#IP = '192.168.0.246' #Emmett’s Robot IP
IP = '192.168.137.120' #Mahshid's Robot IP
port = 5000
marker_dimension = 0.046  #4.6 centimeter before
worldx = 889 #508#-marker_dimension*1000 #millimeters
worldy = 508 #401#-marker_dimension*1000 #millimeters
# modify these based on your enviornment settings
robot_ID = 7
bottom_left = 31  #this is the origin - positivex: towards bottom right - positivey: towards top left
bottom_right = 32
top_left = 9
top_right = 20

def UDP(IP,port,message):
   sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #IPv4 DNS server - UDP protocol
   sock.sendto(bytes(message, "utf-8"), (IP,port)) #self, data, address

def getMarkerCenter(corners):
 px = (corners[0][0] + corners[1][0] + corners[2][0]+ corners[3][0]) * 0.25
 py = (corners[0][1] + corners[1][1] + corners[2][1]+ corners[3][1]) * 0.25
 return [px,py]

def getMarkerRotation(corners):
 unit_x_axis = [1.,0.]
 center = getMarkerCenter(corners)
 right_edge_midpoint = (corners[1]+corners[2])/2.
 unit_vec = (right_edge_midpoint-center)/np.linalg.norm(right_edge_midpoint-center)
 angle = np.arccos(np.dot(unit_x_axis,unit_vec))
 return angle

def inversePerspective(rvec, tvec):
   R, _ = cv2.Rodrigues(rvec)
   R = np.array(R).T #this was np.matrix but had error
   invTvec = np.dot(-R, np.array(tvec))
   invRvec, _ = cv2.Rodrigues(R)
   return invRvec, invTvec
def normalize(v):
   if np.linalg.norm(v) == 0 : return v
   return v / np.linalg.norm(v)

"""" the function gets the corners of an aruco marker in the camera space as the origin with its
X (Green) and Y (Red) axis. The origin is the bottom left corner, the coordinate of the point is calculated
in relation to this origin """
def findWorldCoordinate(originCorners, point):
   zero = np.array(originCorners[3]) #bottom left as the origin - check the data structure
   x = (np.array(originCorners[0]) - zero)  # bottom right - Green Axis -- throw out z
   y = (np.array(originCorners[1]) - zero)   # top left - Red Axis -- throw out z
   x = x[0][0:2]
   y = y[0][0:2]
   x = normalize(x)
   y = normalize(y)
   #print("x", x)
   vec = (point - zero)[0][0:2]
   #print("vec", vec)
   vecNormal = normalize(vec)
   cosX = np.dot(x,vecNormal)
   cosY = np.dot(y,vecNormal)
   xW = np.linalg.norm(vec) * cosX
   yW = np.linalg.norm(vec) * cosY
   return [xW, yW]
################################
try:
   # Getting the camera calibration information
   path = r'/home/pi/ECE-5725/camera_codes/picamera.yml'
   cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_READ)
   new_matrix = cv_file.getNode("new_matrix").mat()
   dist_matrix = cv_file.getNode("distortion_coef").mat()
   mtx = cv_file.getNode("matrix").mat()
   cv_file.release()

   found_dict_pixel_space = {}
   found_dict_camera_space = {}
   found_dict_world_space = {}
   found_dict_homography_space = {}
   final_string = ""
   originRvec = np.array([0,0,1])
   markerRvec= np.array([0,0,0])

 # read aruco markers and create dictionary
   aruco_source_path = glob.glob(r'/home/pi/ECE-5725/camera_codes/aruco/*.jpg', )
   aruco_source = []
   for ar in aruco_source_path:
       im_src = cv2.imread(ar)
       aruco_source.append(im_src)
   aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
  
   cap = cv2.VideoCapture(0)
   #pool = mp.Pool(mp.cpu_count()) # init pool for multiprocessing
  
   while True:
       t0 = time.time()
       #ret, frame = cap.read() # read frame
       ret, image = cap.read() # read frame
       #cv2.imshow('frame', frame)
       t1 = time.time()-t0
       #----aruco detection-------------------
       gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # converts image color space from BRG to grayscale
       t2 = time.time()-t0
       data = aruco.detectMarkers(gray, aruco_dict) # detect aruco markers
      
       t3 = time.time()-t0
       corners = data[0] # corners of found aruco markers
       ids = data[1] # ids of found aruco markers
       originIDglobal = 0
       if np.all(ids): # if any markers were found
           #print("found one!")
           t4 = time.time()-t0
           result = aruco.estimatePoseSingleMarkers(corners, marker_dimension, new_matrix, dist_matrix) # estimate the pose of the markers
           rvecs = result[0] # rotation vectors of markers
           tvecs = result[1] # translation vector of markers
           imageCopy = image

           #setting bottom_left as the origin
           if bottom_left in ids:
               originID = np.where(ids == bottom_left)[0][0]
               originIDglobal = originID
           else:
               originID = originIDglobal
           originCorners = corners[originID] # corners of the tag set as the origin
           OriginCornersCamera = getCornerInCameraWorld(marker_dimension, rvecs[originID], tvecs[originID])[0] # origin tag corners in camera space
           originRvec = rvecs[originID] # rotation vec of origin tag
           originTvec = tvecs[originID] # translation vec of origin tag
          
           display = cv2.aruco.drawDetectedMarkers(imageCopy,corners,ids) # display is image copy with boxes drawn around the tags
           t5 = time.time()-t0
           for i in range(len(ids)): # for each tag found in image
               ID = ids[i]
               rvec = rvecs[i]
               tvec = tvecs[i]
               corners4 = corners[i]
              
               display = cv2.aruco.drawAxis(imageCopy,new_matrix,dist_matrix,rvec,tvec,0.03) # draw 3d axis, 3 centimeters
               found_dict_pixel_space[""+str(ids[i][0])] = corners4 # put the corners of this tag in the dictionary
          
           # Homography
           zero = found_dict_pixel_space[str(bottom_left)][0][3] #bottom left - 3
           x = found_dict_pixel_space[str(bottom_right)][0][2] #bottom right - 27
           y = found_dict_pixel_space[str(top_left)][0][0] #top left - 22
           xy = found_dict_pixel_space[str(top_right)][0][1] #top right - 24

           workspace_world_corners = np.array([[0.0, 0.0], [worldx, 0.0], [0.0, worldy], [worldx, worldy]], np.float32) # 4 corners in millimeters
           workspace_pixel_corners = np.array([zero,x,y,xy], np.float32)  # 4 corners in pixels

           # Homography Matrix
           h, status = cv2.findHomography(workspace_pixel_corners, workspace_world_corners) #perspective matrix
           t6=time.time()-t0
           im_out = cv2.warpPerspective(image, h, (worldx,worldy)) #showing that it works
          
           t7 = time.time()-t0
           for i in range(len(ids)):
               j = ids[i][0]
               corners_pix = found_dict_pixel_space[str(j)]#[0]
               corners_pix_transformed = cv2.perspectiveTransform(corners_pix,h)
               found_dict_homography_space[str(j)] = corners_pix_transformed
           print(found_dict_homography_space)
           robot = found_dict_homography_space[str(robot_ID)][0]
           print(getMarkerCenter(robot))
           cv2.imshow('Warped Source Image', im_out)
           t8=time.time()-t0
           print("t1: %8.4f   t2: %8.4f   t3: %8.4f   t4: %8.4f   t5: %8.4f   t6: %8.4f   t7: %8.4f   t8: %8.4f" %(t1,t2-t1,t3-t2,t4-t3,t5-t4,t6-t5,t7-t6,t8-t7))
      
       else:
           display = image
           cv2.imshow('Display', display)

       ###### sending the data to the robot ########################
       robot_center = getMarkerCenter(robot)
       robot_angle = getMarkerRotation(robot)
       #UDP(IP, port, str([robot_center[0], robot_center[1],robot_angle,worldx,worldy]))
       UDP(IP, port, str([robot_center[0], robot_center[1], robot[1][0], robot[1][1], robot[0][0], robot[0][1], worldx, worldy]))

       if cv2.waitKey(1) & 0xFF == ord('q'):
           break

except Exception as e:
 print(e)

# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()

# get current robot coords from camera
  raw_data, address = s.recvfrom(4096)
  #print("\n\n 2. Server received: ", data.decode('utf-8'), "\n\n")
  data=raw_data.decode('utf-8')
  data = data[1:-1]
  data = data.split(',')
  
  center_x, center_y,right_x,right_y,left_x,left_y,world_x, world_y  = map(float,data) #UDP data

def calcDiffSteering(waypoint,robot_center,robot_angle,epsilon):
  goal_velocity = [waypoint[0]-robot_center[0],waypoint[1]-robot_center[1]]
  max_abs_goal_velocity = max(abs(goal_velocity[0]),abs(goal_velocity[1]))
  goal_velocity = [goal_velocity[0]/max_abs_goal_velocity,goal_velocity[1]/max_abs_goal_velocity]
  wheel_vels = calcWheelVels(goal_velocity,robot_angle,epsilon)
  return wheel_vels

def robotSteering(l_level,r_level,speed=100.):
  # speed range 0-100
  global l_pwm,r_pwm
  l_abs = abs(l_level)
  r_abs = abs(r_level)
  fastest = max(l_abs,r_abs)
  if l_level >= 0:
    ccw(l_pwm,l_level/fastest*speed)
  else:
    cw(l_pwm,abs(l_level)/fastest*speed)
  if r_level >= 0:
    cw(r_pwm,r_level/fastest*speed)
  else:
    ccw(r_pwm,abs(r_level)/fastest*speed)

epsilon = 150

 wheel_vels = calcDiffSteering(lines[current_line][current_point],robot_loc,robot_angle,epsilon)
  robotSteering(wheel_vels[0],wheel_vels[1],10)

def moveTowardsGoal(right_distance, left_distance, center_distance, t):
 global speed

 #faced totally opposite from goal
 if (center_distance<right_distance and center_distance<left_distance):
    print("Backwards!")
    if right_distance>left_distance:
      robotFastLeft(speed)
      print("FL back")
    else:
      robotFastRight(speed)
      print ("FR back")

 # faced towards goal
 else:
   if (right_distance>left_distance and right_distance - left_distance > 3*t):
     robotFastLeft(speed)
     print("FL")
   elif (right_distance>left_distance and right_distance - left_distance > t):
     robotLeft(speed)
     print("L")
   elif (left_distance>right_distance and left_distance - right_distance > 3*t):
     robotFastRight(speed)
     print("FR")
   elif (left_distance>right_distance and left_distance - right_distance > t):
     robotRight(speed)
     print("R")
   else:
     print("Forwards")
     robotForward(speed)

def robotRight(speed=100.):
   global l_pwm,r_pwm
   ccw(l_pwm,speed*1.)
   cw(r_pwm,speed*0.5)

def robotLeft(speed=100.):
   global l_pwm,r_pwm
   ccw(l_pwm,speed*0.25)
   cw(r_pwm,speed*1.)

def robotFastRight(speed=100):
   global l_pwm,r_pwm
   ccw(l_pwm,speed*1.)
   cw(r_pwm,speed*0.25)

def robotFastLeft(speed=100):
   global l_pwm,r_pwm
   cw(l_pwm,speed*0.25)
   cw(r_pwm,speed*1.)

def calculateAngle(right,left,center,goal):
  cen2_x = (right[0] + left[0])*0.5
  cen2_y = (right[1] + left[1])*0.5
  rob_direction = np.array([cen2_x - center[0], cen2_y - center[1]])
  rob_direction = normalize(rob_direction)
  goal_direction = np.array([goal[0] - center[0], goal[1] - center[1]])
  goal_direction = normalize(goal_direction)
  print("rob direction",rob_direction)
  print("gaol direction",goal_direction)
  #theta = np.arccos(np.dot(rob_direction, goal_direction) / (np.linalg.norm(rob_direction) * np.linalg.norm(goal_direction)))
  #theta = np.arccos(np.dot(rob_direction, goal_direction))
  theta = np.arctan2(goal_direction[1],goal_direction[0]) - np.arctan2(rob_direction[1],rob_direction[0])
  if theta>np.pi:
    theta = theta-2*np.pi
  elif theta <= -np.pi:
    theta = theta + 2*np.pi
  return theta

def robotPIDTurning(steering,speed):
   global l_pwm,r_pwm
   l_steer = 0.5+steering
   r_steer = 0.5-steering
   if l_steer>=0.:
       ccw(l_pwm,speed*min(1.,l_steer))
   elif l_steer<0:       cw(l_pwm,speed*abs(max(-1.,l_steer)))
   if r_steer>=0.:
       cw(r_pwm,speed*min(1.,r_steer))
   elif r_steer<0:    ccw(r_pwm,speed*abs(max(-1.,r_steer)))
  robot_loc = [center_x,center_y]
  goal_point = tftToCameraCoords(lines[current_line][current_point],[width,height],[world_x,world_y])
  right_distance = distance(goal_point[0],goal_point[1],right_x,right_y)
  left_distance = distance(goal_point[0],goal_point[1],left_x,left_y)
  center_distance = distance(goal_point[0],goal_point[1],center_x, center_y)
  angle = calculateAngle([left_x,left_y],[right_x,right_y],[center_x,center_y],goal_point)
  if not angle_idx_filled:
    previous_angle = angle
    d_angle = 0
    i_angle = 0
  else:
    d_angle = angle-previous_angle
    previous_angle = angle
    i_angle = i_angle+angle
  print(angle)
  
  if closeToPoint(goal_point,robot_loc,point_distance_threshold): # check if robot is close to current goal point
    print("I'm close")
    print ("number of lines: ", len(lines))
    print ("number of points: ", len(points))
    if current_point == 0:
      ### put pen down ******************
      penDown()
    current_point = current_point + 1 #change later
    i_angle = angle
    if current_point >= len(lines[current_line])-1:
      current_line = current_line +1
      current_point = 0
      ### lift pen **************************
      penUp()
      if current_line >= len(lines)-1:
        print("all done")
        all_lines_drawn = True
        robotStop()
   
  steering = (-(p*angle + i*i_angle + d*d_angle))%(2*np.pi)
  print("steering: ", steering)
  robotPIDTurning(steering,speed)